/**
 * @file custom_control.c
 * @author 学号-姓名
 * @brief "custom_control.dll" 动态链接库源文件
 *
 * 若选择实验B，这个文件将贯彻整个课程实验，最后形成一个含PID、超前滞后、低通滤波器、干扰观测器等常用控制算法的库
 *
 * @version 0.1
 * @date 你的最后更改日期
 *
 * @copyright Copyright (c) 2022
 */
#include "custom_control.h"

/**
 * @brief 实验B：自定义的PID数据结构
 *
 * 1. 运行时该类型的变量以指针的形式传入custom_pid_control函数，你对它的修改会保持下去
 * 2. 结构体长度不可超过256字节，否则将内存访问越界
 */
typedef struct
{

} PidData;

/**
 * @brief 实验B：自定义的DOB数据结构
 *
 * 1. 运行时该类型的变量以指针的形式传入custom_dob_control函数，你对它的修改会保持下去
 * 2. 结构体长度不可超过256字节，否则将内存访问越界
 */
typedef struct
{
  TF LPF_1;
  TF LPF_2;
  TF LPF_3;
  TF Plant_1;
  TF Plant_2;
  TF Plant_3;
  double prev_u0;
} DobData;

/**
 * @brief 实验B：自定义的超前滞后控制器数据结构
 *
 * 1. 运行时该类型的变量以指针的形式传入custom_lead_lag_control函数，你对它的修改会保持下去
 * 2. 结构体长度不可超过256字节，否则将内存访问越界
 */

typedef struct
{
  TF LeadLagModule[10];
} LeadLagData;

/**
 * @brief 实验B：自定义的PID算法
 *
 * @param r 指令 [deg]
 * @param y 反馈 [deg]
 * @param dt 控制周期 [s]
 * @param handle 包含Kp、Ki、Kd，以及指向一段256字节内存的指针
 * @return double 输出的控制量 [V]
 */

double custom_pid_control(double r, double y, double dt, PidHandle handle)
{
  // 将指向128字节内存的指针强转为指向PidData的指针
  PidData *pid_data = (PidData *)handle.data;

  return 0;
}

/**
 * @brief 实验B：自定义的PID算法重置函数
 *
 * 在控制参数更新时被调用，可以把历史信息清零，比如积分值
 *
 * @param handle 包含Kp、Ki、Kd，以及指向一段256字节长度内存的指针
 */

void custom_pid_reset(PidHandle handle)
{
  // 将指向256字节内存的指针强转为指向PidData的指针
  PidData *pid_data = (PidData *)handle.data;
}

/**
 * @brief 实验B：编写两种形式的传递函数
 *
 * 作为基本单元，用于计算整个DOB中所需的各个传函模块的输出值
 *
 **/

/**
 *                                                      K(a*s + 1)
 * @brief 函数custom_transfer_function用于计算 G(s) = ---------------形式的传递函数输出
 *                                                       b*s + 1
 * @param u 当前拍的传函输入
 * @param K 传函分子上的增益
 * @param a 传函分子时间常数
 * @param b 传函分母时间常数
 * @param dt 离散化周期
 * @param tf 操作的传函对象
 * @return double 返回该传函模块的输出值
 **/

double custom_transfer_function(double u, double K, double a, double b, double dt, TF *tf)
{

  return 0;
}

/**
 *                                                  s
 * @brief 函数custom_derivative用于计算 G(s) = ---------------形式的传递函数输出
 *                                                a*s + 1
 * @param u 当前拍的传函输入
 * @param a 传函分母时间常数
 * @param dt 离散化周期
 * @param tf 操作的传函对象
 * @return double 返回该传函模块的输出值
 **/

double custom_derivative(double u, double a, double dt, TF *tf)
{

  return 0;
}

/**
 * @brief 实验B：自定义的DOB（干扰观测器）控制算法
 *        以下给出了二阶对象+二阶干扰观测器的程序，辨识对象为三阶的同学们可以参考该程序，稍作修改实现相同的功能
 *
 * @param u 当前拍DOB输入，即PID控制器的输出
 * @param y 当前拍系统输出
 * @param dt 采样时间
 * @param handle 包括DOB时间常数tau，名义对象增益K，时间常数T1、T2，以及指向一段256字节长度内存的指针
 * @return double DOB输出值，即实际控制量
 **/

double custom_dob_control(double u, double y, double dt, DobHandle handle)
{
  DobData *dob_data = (DobData *)handle.data;
  double pre_u0 = dob_data->prev_u0;
  double tau = handle.tau;
  double K = handle.K;
  double T1 = handle.T1;

  double LPF_1_out = custom_transfer_function(pre_u0, 1, 0, tau, dt, &dob_data->LPF_1);
  double LPF_2_out = custom_transfer_function(LPF_1_out, 1, 0, tau, dt, &dob_data->LPF_2); // calc data 1
  double Plant_1_out = custom_derivative(y, tau, dt, &dob_data->Plant_1);
  double Plant_2_out = custom_transfer_function(Plant_1_out, 1 / K, T1, tau, dt, &dob_data->Plant_2); // calc data 2
  double u0 = u - (Plant_2_out - LPF_2_out);
  dob_data->prev_u0 = u0;
  return u0;
}

/**
 * @brief 实验B：自定义的DOB算法重置函数
 *        在控制参数更新时被调用，可以把历史信息清零
 *
 * @param handle 包括DOB时间常数tau，名义对象增益K，时间常数T1、T2，以及指向一段256字节长度内存的指针
 **/

void custom_dob_reset(DobHandle handle)
{
  DobData *dob_data = (DobData *)handle.data;
  dob_data->prev_u0 = 0;
  dob_data->LPF_1.prev_u = 0;
  dob_data->LPF_1.prev_y = 0;
  dob_data->LPF_2.prev_u = 0;
  dob_data->LPF_2.prev_y = 0;
  dob_data->Plant_1.prev_u = 0;
  dob_data->Plant_1.prev_y = 0;
  dob_data->Plant_2.prev_u = 0;
  dob_data->Plant_2.prev_y = 0;
}

/**
 * @brief 实验B：自定义的超前滞后控制器
 *
 * @param r 指令 [deg]
 * @param y 反馈 [deg]
 * @param dt 控制周期 [s]
 * @param handle 包含K，tau1，tau2三个长度为10的数组，以及指向一段256字节内存的指针
 * @return double 输出的控制量 [V]
 */

double custom_lead_lag_control(double r, double y, double dt, LeadLagHandle handle)
{
  LeadLagData *lead_lag_data = (LeadLagData *)handle.data;

  return 0;
}

/**
 * @brief 实验B：自定义的超前滞后控制器重置函数
 *        在控制参数更新时被调用，可以把历史信息清零
 *
 * @param handle 包括三个数组K，tau1，tau2，以及指向一段256字节长度内存的指针
 **/

void custom_lead_lag_reset(LeadLagHandle handle)
{
  LeadLagData *lead_lag_data = (LeadLagData *)handle.data;
}

/**
 * @brief 实验B：自定义的双闭环PID算法
 *
 * @param r_pos 位置指令 [deg]
 * @param r_vel 一个指向速度指令值的指针，用于实验系统绘制波形[deg/s]
 * @param y_vel 速度反馈 [deg/s]
 * @param y_pos 位置反馈 [deg]
 * @param dt 控制周期 [s]
 * @param outer_handle 包含位置环的Kp、Ki、Kd，以及指向一段256字节内存的指针
 * @param inner_handle 包含速度环的Kp、Ki、Kd，以及指向一段256字节内存的指针
 * @return double 输出的控制量 [V]
 */

double custom_dual_pid_control(double r_pos, double *r_vel, double y_vel, double y_pos, double dt, PidHandle outer_handle, PidHandle inner_handle)
{

  return 0;
}

/**
 * @brief 实验B：自定义的双闭环PID算法重置函数
 *
 * 在控制参数更新时被调用，可以把历史信息清零，比如积分值
 *
 * @param outer_handle 包含位置环的Kp、Ki、Kd，以及指向一段256字节内存的指针
 * @param inner_handle 包含速度环的Kp、Ki、Kd，以及指向一段256字节内存的指针
 */

void custom_dual_pid_reset(PidHandle outer_handle, PidHandle inner_handle)
{
  PidData *outer_pid_data = (PidData *)outer_handle.data;
  PidData *inner_pid_data = (PidData *)inner_handle.data;
}